/*Copyright © 2010 by Boris Avdeev. All rights reserved.

*/

//AFT length forward model based on Ketcham's papers.
//TODO: Hard-coded constants. No multi-kinetics.


#include <math.h>

/* Fill in some defs from R that aren't in math.h */
#ifndef M_PI
#define M_PI 3.141592653589793238462643383280
#endif
#define M_LN_SQRT_2PI 0.918938533204672741780329736406
#define M_LN_SQRT_PId2  0.225791352644727432363097614947
#define M_1_SQRT_2PI  0.39894228040143267793994605993
#define M_2PI   6.28318530717958647692528676655
#define M_SQRT_32 5.656854249492380195206754896838

//using namespace std;


const double a  =  0.35;
const double b  =  2.7;
const double c0 = -4.87;
const double c1 =  0.000168;
const double c2 = 28.12;


inline double dnorm(double x, double mean, double sd){
  // Normal PDF
  double X = (x - mean) / sd;
  return (M_1_SQRT_2PI * exp(-0.5 * X * X) / sd);
}

inline double t_eq (double r, double T){
  // Equivalent time s(1,K)
  return exp(((  pow((1 - pow(r,b))/b ,a) - 1)/a - c0)/c1/T - c2);
}

inline double r_fa (double t,double T) {
  // Fanning Arrhenius model  1(s,K)
  return pow( 1-b*pow(a*(c0 + c1*T*(log(t)+c2))+1, 1/a) , 1/b);
}
void rlen(const double* dt, const double* T, const int* nT, double* r){
  // Computes length reduction l/l0
  // given len(Ts) dt-long steps from past to present
  double dt_s = *dt * 1000000 * 31556926; //to seconds
  double teq = 0;
  int i=*nT-1;
  double T_K=T[i]+273.15;
  for (; i>=0; --i){
    r[i] = r_fa(dt_s+teq, T_K);
    if(isnan(r[i]))r[i]=0;
    if(i>0){
      T_K = T[i-1]+273.15;
      teq = t_eq(r[i], T_K);
    }
  }
}

inline double kern_h(double r, double hmult, int varh, int cproj){
  // Kernel DF.
  //kernel width
  double h=0.1;
  if(varh){
    //h is computed according to Ketcham2005, fig.6,
    //approximately replacing l with r
    if(cproj){
      double l = 16.3 * r;
      h = 0.01*l*l - 0.2827*l + 2.501;
    }
    //else h = 0.02858*l*l - 0.8733*l + 7.464;
    else{//h is computed according to Lutz1991, app
      if(r > 0.68) h = 1.32-0.47*r;
      else if(r < 0.43) h = 3;
      else h = 6.5-8.1*r;
    }
  }
  return h*hmult;
}

void drlen(const double* x, const int* nx, const double* r, const int* nr, double* d,
	    const int* cproj, const double* hmult, const int* varh, const int* gkdeb){
  //computes PDF of r at x
  if(*gkdeb){ //bound estimate (matlab gkdeb)
    double uB=1;
    double lB=0;
    double rt[*nr];
    double dxdz[*nr];
    double h[*nr];
    int i;
    for(i=0; i<*nr; ++i){
      rt[i] = log((r[i]-lB)/(uB-r[i]));
      dxdz[i] = (uB-lB)/(r[i]-lB)/(uB-r[i]);
      h[i] = kern_h(r[i],*hmult,*varh,*cproj);
    }
    int j;
    for(j=0; j<*nx; ++j){
      double xt = log((x[j]-lB)/(uB-x[j]));
      d[j]=0;
      double nr_obs=0;
      int i;
      for(i=0; i<*nr; ++i){
	if(r[i]>0.13){   //observ. bias Ketcham2000, p.6
	  d[j]+=exp( -pow((xt-rt[i])/h[i],2)/2  )*dxdz[i] / h[i];
	  nr_obs++;
	}
      }
      d[j] = d[j]/(nr_obs * 2.506628274);
    }  
  }
  else{
    double nr_obs=0;
    int i;
    for(i=0; i<*nr; ++i){
      if(r[i]>0.13){   //observ. bias Ketcham2000, p.6
	nr_obs++;
	double h = kern_h(r[i],*hmult,*varh,*cproj);
	int j;
	for(j=0; j<*nx; ++j){
	  if(i==0)d[j]=0; //initialize
	  d[j]+=dnorm(x[j], r[i], h);
	}
      }
    }
    int j;
    for(j=0; j<*nx; ++j){
      d[j] = d[j]/nr_obs;
    }
  }
}

void rlen_loglike(const double* r_data, const int* nr_data, const double* dt, const double* T, const int* nT, double* ll,
		  const int* cproj, const double* hmult, const int* varh, const int* gkdeb){
  //Log-Likelihood for MCMC
  double r_mod[*nT];
  rlen(dt, T, nT, r_mod);
  double dens[*nr_data];
  drlen(r_data,nr_data,r_mod,nT,dens,cproj,hmult,varh,gkdeb);
  int i;
  (*ll)=0;
  for(i=0;i<*nr_data;++i) (*ll)+=log(dens[i]);
}
